using System;
using Microsoft.SPOT;
using GHIElectronics.NETMF.FEZ;
using System.Threading;
using GHIElectronics.NETMF.Hardware;

namespace MLRobotic.IA.Drivers
{
    public class Servomoteur
    {
         #region Attributes
        private PWM servo;
        public Robot.Actionneurs.EtatServo _etat;
        private FEZ_Pin.PWM pWM;
        private uint ouvert;
        private uint milieu;
        private uint ferme;
        #endregion

        #region Properties
        public Robot.Actionneurs.EtatServo CurrentAngle
        {
            get { return _etat; }
        }
        public static DateTime waitingTime = DateTime.Now;
        public const int DelayMouvementServo = 1200;
        #endregion

        #region Constructor
        public Servomoteur(FEZ_Pin.PWM servopin)
        {
            _etat = 0;
            servo = new PWM((PWM.Pin)servopin);
            this.SetMiddle();
        }

        public Servomoteur(FEZ_Pin.PWM servopin, int position)
        {
            _etat = 0;
            servo = new PWM((PWM.Pin)servopin);
        }

        public Servomoteur(FEZ_Pin.PWM pWM, uint ouvert, uint milieu, uint ferme)
        {
            // TODO: Complete member initialization
            this.pWM = pWM;
            this.ouvert = ouvert;
            this.milieu = milieu;
            this.ferme = ferme;
            servo = new PWM((PWM.Pin)pWM);
            _etat = Robot.Actionneurs.EtatServo.Desactive;
        }


        #endregion

        #region Methods public
        public void Check()
        {
            SetMin();
            SetMax();
            SetMiddle();
        }

        public void GoAngle(uint angle)
        {
            servo.SetPulse(20 * 1000 * 1000, angle * 1000);

        }

        public void SetMin()
        {
            servo.SetPulse(20 * 1000 * 1000, 500 * 1000);
        }

        public void SetMax()
        {
            servo.SetPulse(20 * 1000 * 1000, 1250 * 1000);
        }

        public void SetMiddle()
        {
            servo.SetPulse(20 * 1000 * 1000, 2500 * 1000);
        }

        public void Scanning()
        {
            Thread.Sleep(100);//wait for a second

            for (uint i = 500; i < 600; i += 5)
            {
                servo.SetPulse(20 * 1000 * 1000, i * 1000);
                Thread.Sleep(3);
            }

            for (uint i = 600; i < 700; i += 5)
            {
                servo.SetPulse(20 * 1000 * 1000, i * 1000);
                Thread.Sleep(2);
            }

            for (uint i = 700; i < 2300; i += 5)
            {
                servo.SetPulse(20 * 1000 * 1000, i * 1000);
                Thread.Sleep(1);
            }

            for (uint i = 2300; i < 2400; i += 5)
            {
                servo.SetPulse(20 * 1000 * 1000, i * 1000);
                Thread.Sleep(2);
            }

            for (uint i = 2400; i < 2500; i += 5)
            {
                servo.SetPulse(20 * 1000 * 1000, i * 1000);
                Thread.Sleep(3);
            }

            Thread.Sleep(100);//wait for a second
                        
            for (uint i = 2500; i > 2400; i -= 5)
            {
                servo.SetPulse(20 * 1000 * 1000, i * 1000);
                Thread.Sleep(3);
            }

            for (uint i = 2400; i > 2300; i -= 5)
            {
                servo.SetPulse(20 * 1000 * 1000, i * 1000);
                Thread.Sleep(2);
            }

            for (uint i = 2300; i > 700; i -= 5)
            {
                servo.SetPulse(20 * 1000 * 1000, i * 1000);
                Thread.Sleep(1);
            }

            for (uint i = 700; i > 600; i -= 5)
            {
                servo.SetPulse(20 * 1000 * 1000, i * 1000);
                Thread.Sleep(2);
            }

            for (uint i = 600; i > 500; i -= 5)
            {
                servo.SetPulse(20 * 1000 * 1000, i * 1000);
                Thread.Sleep(3);
            }

            Thread.Sleep(100);//wait for a second
        }
        #endregion

        #region Methods private
        #endregion

        internal void Move(Robot.Actionneurs.EtatServo mouvement, bool attentefin)
        {
            if (_etat != mouvement)
            {
                if (Robot.isSimulateur)
                    Program.comAsserv.Send(190, (short)mouvement, Drivers.COM.typePacket.SimuBougeServo);
                else
                {
                    switch (mouvement)
                    {

                        case Robot.Actionneurs.EtatServo.Ferme:
                            GoAngle(ferme);
                            break;
                        case Robot.Actionneurs.EtatServo.Ouvert:
                            GoAngle(ouvert);
                            break;
                        case Robot.Actionneurs.EtatServo.Milieu:
                            GoAngle(milieu);
                            break;
                    }
                }
                Log.Write("SRV : Mouvement de servo #" + 190 + " a la position : " + mouvement);
                _etat = mouvement;

                if ((attentefin) && (waitingTime < DateTime.Now.AddMilliseconds(DelayMouvementServo)))
                {

                    Robot.currentAction = Robot.TypeAction.Servo;
                    waitingTime = DateTime.Now.AddMilliseconds(DelayMouvementServo);
                    Log.Write("SRV : Attente mouvement du servo jusqu'a : " + waitingTime.ToString("ss:fff") + ", il est: " + DateTime.Now.ToString("ss:ff"));
                }
            }
        }

        public static void Process()
        {
            if (DateTime.Now > waitingTime && Robot.currentAction == Robot.TypeAction.Servo)
            {
                Log.Write("SRV : Fin attente servo : " + DateTime.Now.ToString("ss:fff"));
                Robot.currentAction = Robot.TypeAction.Arret;
            }
        }
    }
    public class MouvementServo
    {
        public Servomoteur _servo;
        public Robot.Actionneurs.EtatServo _typeMouvement;
        public bool _attentefin;
        public Robot.Actionneurs.ServoConditionnel _servoConditionnel = Robot.Actionneurs.ServoConditionnel.none;

        public MouvementServo(Servomoteur servo, Robot.Actionneurs.EtatServo typeMouvement, bool attentefin)
        {
            _servo = servo;
            _typeMouvement = typeMouvement;
            _attentefin = attentefin;

        }

        public MouvementServo(Servomoteur servo, Robot.Actionneurs.EtatServo typeMouvement)
            : this(servo, typeMouvement, false)
        { }

        public MouvementServo(Robot.Actionneurs.ServoConditionnel servoConditionnel, Robot.Actionneurs.EtatServo typeMouvement)
            : this(servoConditionnel, typeMouvement, false)
        { }

        public MouvementServo(Robot.Actionneurs.ServoConditionnel servoConditionnel, Robot.Actionneurs.EtatServo typeMouvement, bool attentefin)
        {
            // TODO: Complete member initialization
            _servoConditionnel = servoConditionnel;
            _typeMouvement = typeMouvement;
            _attentefin = attentefin;
        }


    }
}
